#include "StdAfx.h"

#include <CryExtension/Impl/ClassWeaver.h>
#include <CryExtension/Impl/ICryFactoryRegistryImpl.h>
#include <CryExtension/CryCreateClassInstance.h>

#include <I3DEngine.h>
#include <IRenderAuxGeom.h>
#include "../CharacterInstance.h"
#include "../Model.h"
#include "../ModelSkeleton.h"
#include "../CharacterManager.h"

#include "PoseModifierHelper.h"
#include "FeetLock.h"

namespace Util 
{
	SPU_NO_INLINE void AdjustFeetHelper(QuatT &rAbsoluteCalf, QuatT &rRelativeCalf, QuatT &rAbsoluteTight, QuatT &rAbsoluteFoot,QuatT &rRelativeFoot, const Vec3 &rCross, const Vec3 &rKneeDir, uint32 iteration, float knee_factor)
	{
		for (uint32 i=0; i<iteration; i++)
		{
			Vec3 ulleg = rAbsoluteCalf.t - rAbsoluteTight.t;
			Vec3 llleg = rAbsoluteFoot.t - rAbsoluteCalf.t;
			Vec3 lcross = ulleg%llleg;
			if ( (lcross|lcross)>0.0001f && (rCross|lcross)>0)
				break;
			rAbsoluteCalf.t += rKneeDir*knee_factor;
		}
		rRelativeCalf = rAbsoluteTight.GetInverted() 	* rAbsoluteCalf;
		rRelativeFoot = rAbsoluteCalf.GetInverted() 	* rAbsoluteFoot;
	}
}

/*

CFeetPoseStore

*/

CRYREGISTER_CLASS(CFeetPoseStore)

//

CFeetPoseStore::CFeetPoseStore()
{
}

CFeetPoseStore::~CFeetPoseStore()
{
}

//

SPU_NO_INLINE void CFeetPoseStore::ComputeFootPositions(QuatT &rRelativeCalf, QuatT &rRelativeTight, QuatT &rFoot, QuatT &plevis, Vec3 &rCross, Vec3 &rKneeDir, QuatT &rAnkle)
{	
	QuatT rAbsoluteTight	= plevis* rRelativeTight;
	QuatT rAbsoluteCalf		= rAbsoluteTight * rRelativeCalf;

	rAnkle = rAbsoluteCalf	* rFoot;	

	Vec3 ulleg = rAbsoluteCalf.t - rAbsoluteTight.t;	
	Vec3 llleg = rAnkle.t - rAbsoluteCalf.t;

	llleg.Normalize();	
	ulleg.Normalize();

	rCross = ulleg%llleg;	
	f32 ldot=rCross|rCross;

	//	assert( ldot>0.0001f ); 

#if !defined(__SPU__)
	/*	if (ldot<0.0001f)
	{
	const char* pAnimName=0;
	CAnimationSet* pAnimationSet = &m_pInstance->m_pModel->m_AnimationSet;
	const ModelAnimationHeader* anim;

	if (m_arrLayer_AFIFO[0][0].m_LMG.m_nLMGID>=0)
	anim	=	&pAnimationSet->GetModelAnimationHeaderRef(m_arrLayer_AFIFO[0][0].m_LMG.m_nLMGID);
	else
	anim	=	&pAnimationSet->GetModelAnimationHeaderRef(m_arrLayer_AFIFO[0][0].m_LMG.m_nAnimID[0]);
	pAnimName=anim->GetAnimName();
	const char* pModelFilepath = m_pInstance->m_pModel->GetFilePath();
	AnimFileWarning(0,"Warning: hyper extension on leg. IK will fail: %s %s", pModelFilepath,pAnimName );
	}*/
#endif // !__SPU__

	Vec3 lkneeaxis	= ulleg%llleg;	
	rKneeDir = (ulleg%lkneeaxis).GetNormalized();	
}

// IAnimationPoseModifier

SPU_INDIRECT(CommandBufferExecute(ML))
bool CFeetPoseStore::Execute(const SAnimationPoseModiferParams& params)
{
	CModelSkeleton* pModelSkeleton	= (CModelSkeleton*)(SPU_MAIN_PTR( params.GetICharacterModelSkeleton() ));
	QuatT* pRelPose = SPU_LOCAL_PTR( params.pPoseRelative );
	QuatT* pAbsPose = SPU_LOCAL_PTR( params.pPoseAbsolute );
	m_pFeetData->m_IsAnkleValid = 0;

	{
		uint32 hinit = pModelSkeleton->m_strFeetLockLIKHandle.size();
		if (hinit==0)
			return 0;
		const char* strLIKSolver=pModelSkeleton->m_strFeetLockLIKHandle.c_str();
		uint64 nHandle = *(uint64*)strLIKSolver;
		int32 idxDefinition = pModelSkeleton->GetLimbDefinitionIdx(nHandle);
		if (idxDefinition < 0)
			return 0;
		const IKLimbType& rIKLimbType = pModelSkeleton->m_IKLimbTypes[idxDefinition];
		uint32 numLinks = rIKLimbType.m_arrRootToEndEffector.size();
		int32 nRootTdx=rIKLimbType.m_arrRootToEndEffector[0];
		QuatT lAnkle = pRelPose[nRootTdx];		
		for (uint32 i=1; i<numLinks; i++)
		{
			int32 cid = rIKLimbType.m_arrRootToEndEffector[i];
			lAnkle=lAnkle*pRelPose[cid];
			lAnkle.q.Normalize();
		}
		m_pFeetData->m_lAnkle = lAnkle;
		assert(m_pFeetData->m_lAnkle.IsValid());
	}

	{
		uint32 hinit = pModelSkeleton->m_strFeetLockRIKHandle.size();
		if (hinit==0)
			return 0;
		const char* strRIKSolver=pModelSkeleton->m_strFeetLockRIKHandle.c_str();
		uint64 nHandle = *(uint64*)strRIKSolver;
		int32 idxDefinition = pModelSkeleton->GetLimbDefinitionIdx(nHandle);
		if (idxDefinition < 0)
			return 0;
		const IKLimbType& rIKLimbType = pModelSkeleton->m_IKLimbTypes[idxDefinition];
		uint32 numLinks = rIKLimbType.m_arrRootToEndEffector.size();
		int32 nRootTdx=rIKLimbType.m_arrRootToEndEffector[0];
		QuatT rAnkle = pRelPose[nRootTdx]; 
		for (uint32 i=1; i<numLinks; i++)
		{
			int32 cid = rIKLimbType.m_arrRootToEndEffector[i];
			rAnkle=rAnkle*pRelPose[cid];
			rAnkle.q.Normalize();
		}
		m_pFeetData->m_rAnkle = rAnkle;
		assert(m_pFeetData->m_rAnkle.IsValid());
	}

	m_pFeetData->m_IsAnkleValid = 1;

#ifdef _DEBUG
	uint32 numJoints=pModelSkeleton->GetJointCount();
	for (uint32 j=0; j<numJoints; j++)
	{
		assert( pRelPose[j].q.IsUnit() );
		assert( pAbsPose[j].q.IsUnit() );
		assert( pRelPose[j].IsValid() );
		assert( pAbsPose[j].IsValid() );
	}
#endif			

	return false;
}

/*

CFeetPoseRestore

*/

CRYREGISTER_CLASS(CFeetPoseRestore)

//

CFeetPoseRestore::CFeetPoseRestore()
{
}

CFeetPoseRestore::~CFeetPoseRestore()
{
}

// IAnimationPoseModifier

SPU_INDIRECT(CommandBufferExecute(ML))
bool CFeetPoseRestore::Execute(const SAnimationPoseModiferParams& params)
{
	if (!m_pFeetData->m_IsAnkleValid)
		return false;

	CModelSkeleton* pModelSkeleton	= (CModelSkeleton*)(SPU_MAIN_PTR( params.GetICharacterModelSkeleton() ));
	QuatT*	const	__restrict	pAbsPose	= SPU_LOCAL_PTR(params.pPoseAbsolute);
	QuatT*	const	__restrict	pRelPose	= SPU_LOCAL_PTR(params.pPoseRelative);

	{
		const char* strLIKSolver=pModelSkeleton->m_strFeetLockLIKHandle.c_str();
		uint64 nHandle = *(uint64*)strLIKSolver;
		int32 idxDefinition = pModelSkeleton->GetLimbDefinitionIdx(nHandle);
		if (idxDefinition < 0)
			return 0;
		assert(m_pFeetData->m_lAnkle.IsValid());
		const IKLimbType& rIKLimbType = pModelSkeleton->m_IKLimbTypes[idxDefinition];
		uint32 numLinks = rIKLimbType.m_arrRootToEndEffector.size();
		int32 lFootParentIdx = rIKLimbType.m_arrRootToEndEffector[numLinks-2];
		int32 lFootIdx = rIKLimbType.m_arrRootToEndEffector[numLinks-1];
		PoseModifierHelper::IK_Solver( PoseModifierHelper::GetModelSkeleton(params), strLIKSolver, m_pFeetData->m_lAnkle,	SPU_LOCAL_PTR(params.pPoseRelative), SPU_LOCAL_PTR(params.pPoseAbsolute));
		pAbsPose[lFootIdx]=m_pFeetData->m_lAnkle;
		pRelPose[lFootIdx]=pAbsPose[lFootParentIdx].GetInverted()*m_pFeetData->m_lAnkle;
	}

	{
		const char* strRIKSolver=pModelSkeleton->m_strFeetLockRIKHandle.c_str();
		uint64 nHandle = *(uint64*)strRIKSolver;
		int32 idxDefinition = pModelSkeleton->GetLimbDefinitionIdx(nHandle);
		if (idxDefinition < 0)
			return 0;
		assert(m_pFeetData->m_rAnkle.IsValid());
		const IKLimbType& rIKLimbType = pModelSkeleton->m_IKLimbTypes[idxDefinition];
		uint32 numLinks = rIKLimbType.m_arrRootToEndEffector.size();
		int32 rFootParentIdx = rIKLimbType.m_arrRootToEndEffector[numLinks-2];
		int32 rFootIdx = rIKLimbType.m_arrRootToEndEffector[numLinks-1];
		PoseModifierHelper::IK_Solver( PoseModifierHelper::GetModelSkeleton(params), strRIKSolver, m_pFeetData->m_rAnkle,	SPU_LOCAL_PTR(params.pPoseRelative), SPU_LOCAL_PTR(params.pPoseAbsolute));
		pAbsPose[rFootIdx]=m_pFeetData->m_rAnkle;
		pRelPose[rFootIdx]=pAbsPose[rFootParentIdx].GetInverted()*m_pFeetData->m_rAnkle;
	}

#ifdef _DEBUG
	uint32 numJoints=pModelSkeleton->GetJointCount();
	for (uint32 j=0; j<numJoints; j++)
	{
		assert( pRelPose[j].q.IsUnit() );
		assert( pAbsPose[j].q.IsUnit() );
		assert( pRelPose[j].IsValid() );
		assert( pAbsPose[j].IsValid() );
	}
#endif			

	return true;
}

/*

CFeetLock

*/

CFeetLock::CFeetLock()
{
	::CryCreateClassInstance<IAnimationPoseModifier>(
		"AnimationPoseModifier_FeetPoseStore", m_store);
	assert(m_store.get());

	CFeetPoseStore* pStore = static_cast<CFeetPoseStore*>(m_store.get());
	pStore->m_pFeetData = &m_feetData;

	::CryCreateClassInstance<IAnimationPoseModifier>(
		"AnimationPoseModifier_FeetPoseRestore", m_restore);
	assert(m_restore.get());

	CFeetPoseRestore* pRestore = static_cast<CFeetPoseRestore*>(m_restore.get());
	pRestore->m_pFeetData = &m_feetData;
}
